/*
 * @Description: 里程计发布，包装成nav_msgs::odometry
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:12:01
 * @LastEditTime: 2021-10-27 12:41:25
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_PUBLISHER_ODOMETRY_PUBLISHER_HPP_
#define LIDAR_SLAM_PUBLISHER_ODOMETRY_PUBLISHER_HPP_

#include <ros/ros.h>
#include <Eigen/Dense>
#include <nav_msgs/Odometry.h>

namespace lidar_slam {
class OdometryPublisher {
private:
	/* data */
	ros::NodeHandle nh_;
	ros::Publisher publisher_;
	nav_msgs::Odometry odometry_;
private:
	void PublishData(const Eigen::Matrix4f& transform_matrix, ros::Time time);
public:
	OdometryPublisher(	ros::NodeHandle& nh, 
						std::string topic_name,
						std::string base_frame_id,
						std::string child_frame_id,
						size_t buff_size	);

	OdometryPublisher() = default;

	void Publish(const Eigen::Matrix4f& transform_matrix, double time);
	void Publish(const Eigen::Matrix4f& transform_matrix);

	bool HasSubscribers();
};
}

#endif